#ifndef _INCL_CPILOT
#define _INCL_CPILOT

#include <mrpt/poses/CPose3D.h>
#include <mrpt/hwdrivers/C2DRangeFinderAbstract.h>
#include "AP_LocalisationWrapper.h"
#include "AP_PiloteHuit.h"
#include "GlueScan.h"
using namespace mrpt;
using namespace mrpt::slam;
using namespace mrpt::math;
using namespace mrpt::utils;
using namespace std;

class CPilot {
public:
	CPilot();
	~CPilot();

	void piloteWpt(CPose3D pose_m, TPoint3D v_m);
	void estimation(CObservation2DRangeScan scan, CPose3D pose_m);
	void piloteHuit(CObservation2DRangeScan scan, CPose3D pose_m, TPoint3D v_m);

	void generateHuitWpts();
  void generateStaticWpts();
	double getPhiC();
	double getThetaC();
	double getPsiC();

	TPoint3D getCurrentWpt();
	AP_LocalisationWrapper APloc;
	AP_PiloteHuit APpilotehuit;
private:

	double phi_c;
	double theta_c;
	double psi_c;
	int cur_wpt;
	vector<TPoint3D> waypts;

	int etape;
	int tour;

	// Estimation de la position
	CPose3D pose_e;// position estimée en cartésiennes sur la carte (x,y,z,yaw,pitch,roll)
	TPoint3D v_e;// vitesse estimée

	TypeScan scanCalao;

	// quels réglages sont les bons?
	int compteur;
	int compteur2;
	int compteur3;
	float perftest;
	float perfprec;
	float kxincr;
	float kvincr;
};

#endif
